import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose, Twist, Vector3
from nav_msgs.msg import Odometry
import tf2_ros
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import math
from sensor_msgs.msg import JointState
import threading
import time


class OdometryPublisher(Node):
    def __init__(self):
        super().__init__('odometry_publisher')
        self.shutdown_event = threading.Event()
        # self.publisher = self.create_publisher(Odometry, 'odom', 10)
        self.subscription = self.create_subscription(
            Twist,
            '/vel_feedback',  # 假设速度话题是 '/cmd_vel'
            self.vel_feedback,
            10
        )
        # self.timer = self.create_timer(0.01, self.publish_odometry)
        # self.timer1 = self.create_timer(1, self.publish_test)
        self.odom_frame_id = 'odom'
        self.child_frame_id = 'base_footprint'
        self.odom_broadcaster = TransformBroadcaster(self)

        # 初始化里程计变量
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        self.vx = 0.0  # 例子中使用的线速度
        self.vtheta = 0.0  # 例子中使用的角速度

        # 创建并初始化发布者成员属性pub_joint_states_
        self.joint_states_publisher_ = self.create_publisher(
            JointState, "joint_states", 10)
        # 初始化数据
        self._init_joint_states()
        self.pub_rate = self.create_rate(30)
        self.thread_ = threading.Thread(target=self._thread_pub, daemon=True)
        self.thread_.start()

    def publish_test(self):
        self.get_logger().info('hlly')

    def publish_odometry(self):
        # 更新里程计信息
        self.x += self.vx * math.cos(self.theta)
        self.y += self.vx * math.sin(self.theta)
        self.theta += self.vtheta

        # 发布TransformStamped消息
        odom_trans = TransformStamped()
        odom_trans.header.stamp = self.get_clock().now().to_msg()
        odom_trans.header.frame_id = self.odom_frame_id
        odom_trans.child_frame_id = self.child_frame_id
        odom_trans.transform.translation.x = self.x
        odom_trans.transform.translation.y = self.y
        odom_trans.transform.rotation.w = math.cos(self.theta / 2)
        odom_trans.transform.rotation.z = math.sin(self.theta / 2)
        self.odom_broadcaster.sendTransform(odom_trans)

        # 发布Odometry消息
        odom_msg = Odometry()
        odom_msg.header.stamp = self.get_clock().now().to_msg()
        odom_msg.header.frame_id = self.odom_frame_id
        odom_msg.child_frame_id = self.child_frame_id
        odom_msg.pose.pose.position.x = self.x
        odom_msg.pose.pose.position.y = self.y
        odom_msg.pose.pose.orientation.w = math.cos(self.theta / 2)
        odom_msg.pose.pose.orientation.z = math.sin(self.theta / 2)
        odom_msg.twist.twist.linear.x = self.vx
        odom_msg.twist.twist.angular.z = self.vtheta

        self.publisher.publish(odom_msg)

    def _init_joint_states(self):
        # 初始左右轮子的速度
        self.joint_speeds = [0.0, 0.0]
        self.joint_states = JointState()
        self.joint_states.header.stamp = self.get_clock().now().to_msg()
        self.joint_states.header.frame_id = ""
        # 关节名称
        self.joint_states.name = ['left_wheel_joint', 'right_wheel_joint']
        # 关节的位置
        self.joint_states.position = [0.0, 0.0]
        # 关节速度
        self.joint_states.velocity = self.joint_speeds
        # 力
        self.joint_states.effort = []

    def vel_feedback(self, msg):
        # 从 Twist 消息中提取速度信息
        linear_speed = msg.linear.x
        angular_speed = msg.linear.y
        self.joint_speeds = [linear_speed, angular_speed]
        self.vx = linear_speed/20  
        self.vtheta = angular_speed/20
        

    def update_speed(self, speeds):
        self.joint_speeds = speeds
        self.vx = speeds[0]/20  
        self.vtheta = speeds[1]/20
        self.get_logger().info('hlly777777777777')
        self.get_logger().info(f"更新线速度为：{self.joint_speeds[0]}")
        self.get_logger().info(f"更新角速度为：{self.joint_speeds[1]}")

    def _thread_pub(self):
        last_update_time = time.time()
        while not self.shutdown_event.is_set():
            delta_time = time.time()-last_update_time
            last_update_time = time.time()
            # 更新位置
            self.joint_states.position[0] += delta_time * \
                self.joint_states.velocity[0]
            self.joint_states.position[1] += delta_time * \
                self.joint_states.velocity[1]
            # 更新速度
            self.joint_states.velocity = self.joint_speeds
            # 更新 header
            self.joint_states.header.stamp = self.get_clock().now().to_msg()
            # 发布关节数据
            self.joint_states_publisher_.publish(self.joint_states)
            self.pub_rate.sleep()

    def destroy(self):
        # 触发线程退出
        self.shutdown_event.set()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = OdometryPublisher()
    # node.update_speed([15.0, -15.0])
    node.update_speed([0.0, 0.0])
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("接收到中断信号，正在关闭节点...")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
